#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "actionlib_test/AddIntsAction.h"
/*  启动方法：
	   roscore
	   rosrun actionlib_test action_server
	   rosrun actionlib_test action_client

    需求:  创建两个ROS节点，服务器和客户端，客户端可以向服务器发送目标数据N（一个整型数据），服务器会计算1到N之间所有整数的和，
	这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的，又已知服务器从接收到请求到产生响应是一个耗时操作，每累
	加一次耗时0.1s，为了良好的用户体验，需要服务器在计算过程中，每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。

    流程:	1.包含头文件;					2.初始化ROS节点;							3.创建NodeHandle;
        	4.创建action服务对象;            5.处理请求,产生反馈与响应;               6.spin().
*/

typedef actionlib::SimpleActionServer<actionlib_test::AddIntsAction> Server;
void cb(const actionlib_test::AddIntsGoalConstPtr &goal,Server* server){
    int num = goal->num;							//获取目标值
    ROS_INFO("目标值:%d",num);
    int result = 0;									//累加并响应连续反馈
    actionlib_test::AddIntsFeedback feedback;		//连续反馈
    actionlib_test::AddIntsResult Result;
    ros::Rate rate(10);								//通过频率设置休眠时间
    for (int i = 1; i <= num && ros::ok(); i++){
        result += i;
        feedback.progress_bar = i / (double)num;	//组织连续数据并发布
        server->publishFeedback(feedback);
        if (server->isPreemptRequested()){
			printf("cancelled!\n");
			Result.result = result;
			server->setPreempted(Result);
            ROS_INFO("最终结果:%d",result);
			return;
		}
        rate.sleep();
    }
    actionlib_test::AddIntsResult r;				//设置最终结果
    r.result = result;
    server->setSucceeded(r);
    ROS_INFO("最终结果:%d",r.result);
}

int main(int argc, char *argv[]){
//设置或读取地域化信息，目的让ros打印出来的日志信息可以显示中文，否则会显示乱码  https://www.runoob.com/cprogramming/c-function-setlocale.html
    setlocale(LC_ALL,"");	
    ROS_INFO("action服务端实现");
    ros::init(argc,argv,"AddInts_server");			// 2.初始化ROS节点;
    ros::NodeHandle nh;								// 3.创建NodeHandle;
    Server server(nh,"addInts",boost::bind(&cb,_1,&server),false);// 4.创建action服务对象;
    server.start();									// 5.处理请求,产生反馈与响应;
    ros::spin();
    return 0;
}

